/* 
 *  Incub - incubate your baby robot
 *  Copyright (C) 2008 Stanislaw Szymczyk
 *  e-mail: sszymczy@gmail.com   www: http://cogville.cog-lab.com
 *
 *  This file is part of Incub.
 *
 *  Incub is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  Incub is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with Incub.  If not, see <http://www.gnu.org/licenses/>.
 * 
 */
 
#include <GL/gl.h>
#include <GL/glut.h>
#include "Drawable.h"
#include "Sphere.h"
#include "Cylinder.h"
#include "Capsule.h"
#include "Box.h"
#include "Plane.h"

/*
 * Compute lookup table of cos and sin values forming a cirle
 *
 * Notes:
 *    It is the responsibility of the caller to free these tables
 *    The size of the table is (n+1) to form a connected loop
 *    The last entry is exactly the same as the first
 *    The sign of n can be flipped to get the reverse loop
 */

static void fghCircleTable(double **sint,double **cost,const int n)
{
    int i;

    /* Table size, the sign of n flips the circle direction */

    const int size = abs(n);

    /* Determine the angle between samples */

    const double angle = 2*M_PI/(double)( ( n == 0 ) ? 1 : n );

    /* Allocate memory for n samples, plus duplicate of first entry at the end */

    *sint = (double *) calloc(sizeof(double), size+1);
    *cost = (double *) calloc(sizeof(double), size+1);

    /* Bail out if memory allocation fails, fgError never returns */

    if (!(*sint) || !(*cost))
    {
        free(*sint);
        free(*cost);
//        fgError("Failed to allocate memory in fghCircleTable");
    }

    /* Compute cos and sin around the circle */

    (*sint)[0] = 0.0;
    (*cost)[0] = 1.0;

    for (i=1; i<size; i++)
    {
        (*sint)[i] = sin(angle*i);
        (*cost)[i] = cos(angle*i);
    }

    /* Last sample is duplicate of the first */

    (*sint)[size] = (*sint)[0];
    (*cost)[size] = (*cost)[0];
}

/*
 * Draws a solid sphere
 */
void glutSolidCapsule(GLdouble radius, GLdouble length, GLint slices, GLint stacks)
{
    int i,j;

    /* Adjust z and radius as stacks are drawn. */

    double z0,z1;
    double r0,r1;

    /* Pre-computed circle */

    double *sint1,*cost1;
    double *sint2,*cost2;

    fghCircleTable(&sint1,&cost1,-slices);
    fghCircleTable(&sint2,&cost2,stacks*2);

    /* The top stack is covered with a triangle fan */

    z0 = 1.0;
    z1 = cost2[(stacks>0)?1:0];
    r0 = 0.0;
    r1 = sint2[(stacks>0)?1:0];
    double half = length/2.0;

    glBegin(GL_TRIANGLE_FAN);

        glNormal3d(0,0,1);
        glVertex3d(0,0,radius + half);

        for (j=slices; j>=0; j--)
        {
            glNormal3d(cost1[j]*r1,        sint1[j]*r1,        z1 + half      );
            glVertex3d(cost1[j]*r1*radius, sint1[j]*r1*radius, z1*radius + half);
        }

    glEnd();

    /* Cover each stack with a quad strip, except the top and bottom stacks */

    for( i=1; i<stacks/2-1; i++ )
    {
        z0 = z1; z1 = cost2[i+1];
        r0 = r1; r1 = sint2[i+1];

        glBegin(GL_QUAD_STRIP);

            for(j=0; j<=slices; j++)
            {
                glNormal3d(cost1[j]*r1,        sint1[j]*r1,        z1       );
                glVertex3d(cost1[j]*r1*radius, sint1[j]*r1*radius, z1*radius + half);
                glNormal3d(cost1[j]*r0,        sint1[j]*r0,        z0       );
                glVertex3d(cost1[j]*r0*radius, sint1[j]*r0*radius, z0*radius + half);
            }

        glEnd();
    }

	glBegin(GL_QUAD_STRIP);
	    for(j=0; j<=slices; j++)
	    {
	        glNormal3d(cost1[j]*r1,        sint1[j]*r1,        z1       );
	        glVertex3d(cost1[j]*r1*radius, sint1[j]*r1*radius, z1*radius - half);
	        glNormal3d(cost1[j]*r1,        sint1[j]*r1,        z1       );
	        glVertex3d(cost1[j]*r1*radius, sint1[j]*r1*radius, z1*radius + half);
	    }
    glEnd();

    for( i=stacks/2; i<stacks-1; i++ )
    {
        z0 = z1; z1 = cost2[i+1];
        r0 = r1; r1 = sint2[i+1];

        glBegin(GL_QUAD_STRIP);

            for(j=0; j<=slices; j++)
            {
                glNormal3d(cost1[j]*r1,        sint1[j]*r1,        z1       );
                glVertex3d(cost1[j]*r1*radius, sint1[j]*r1*radius, z1*radius - half);
                glNormal3d(cost1[j]*r0,        sint1[j]*r0,        z0       );
                glVertex3d(cost1[j]*r0*radius, sint1[j]*r0*radius, z0*radius - half);
            }

        glEnd();
    }
    
    /* The bottom stack is covered with a triangle fan */

    z0 = z1;
    r0 = r1;

    glBegin(GL_TRIANGLE_FAN);

        glNormal3d(0,0,-1);
        glVertex3d(0,0,-radius - half);

        for (j=0; j<=slices; j++)
        {
            glNormal3d(cost1[j]*r0,        sint1[j]*r0,        z0 - half      );
            glVertex3d(cost1[j]*r0*radius, sint1[j]*r0*radius, z0*radius - half);
        }

    glEnd();

    /* Release sin and cos tables */

    free(sint1);
    free(cost1);
    free(sint2);
    free(cost2);
}

template<>
void DrawableShape<Sphere>::draw() {
	Sphere* thiz = dynamic_cast<Sphere*>(this);
	glutSolidSphere(thiz->getRadius(),10,10);
}

template<>
void DrawableShape<Cylinder>::draw() {
	Cylinder* thiz = dynamic_cast<Cylinder*>(this);
	GLUquadric* quad;

	quad = gluNewQuadric();
	glTranslatef(0, 0, -thiz->getLength() / 2);
	gluCylinder(quad, thiz->getRadius(), thiz->getRadius(), thiz->getLength(), 32, 1);		
	gluDeleteQuadric(quad);

	quad = gluNewQuadric();
	gluQuadricOrientation(quad, GLU_INSIDE);
	gluDisk(quad, 0, thiz->getRadius(), 32, 4);
	gluDeleteQuadric(quad);

	quad = gluNewQuadric();
	glTranslatef(0, 0, thiz->getLength());
	gluQuadricOrientation(quad, GLU_OUTSIDE);
	gluDisk(quad, 0, thiz->getRadius(), 32, 4);
	gluDeleteQuadric(quad);
}

template<>
void DrawableShape<Capsule>::draw() {
	Capsule* thiz = dynamic_cast<Capsule*>(this);
	glutSolidCapsule(thiz->getRadius(), thiz->getLength(), 32,32);
	
/*	glTranslatef(0, 0, -thiz->getLength() / 2);
	GLUquadric* quad = gluNewQuadric();
	gluCylinder(quad, thiz->getRadius(), thiz->getRadius(), thiz->getLength(), 32, 1);		
	gluDeleteQuadric(quad);

	glutSolidSphere(thiz->getRadius(),32,32);
	glTranslatef(0, 0, thiz->getLength());
	glutSolidSphere(thiz->getRadius(),32,32);
*/
}

template<>
void DrawableShape<Box>::draw() {
	Box* thiz = dynamic_cast<Box*>(this);
	glScalef(thiz->getWidth(), thiz->getHeight(), thiz->getLength());
	glutSolidCube(1.0);
}

template<>
void DrawableShape<Plane>::draw() {
	Plane* thiz = dynamic_cast<Plane*>(this);
	glScalef(thiz->getWidth(), 1.0, thiz->getLength());
	glBegin(GL_QUADS);
		glNormal3f(0, 1, 0);
		glVertex3f(-0.5f,0.0f, 0.5f);
		glVertex3f( 0.5f,0.0f, 0.5f);
		glVertex3f( 0.5f,0.0f,-0.5f);
		glVertex3f(-0.5f,0.0f,-0.5f);
	glEnd(); 
}
